#include "car.h"
#include "stdio.h"
#include "math.h"
#include "string.h"
#include "cmsis_os.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "globles.h"
#include "stm32f1xx_hal_tim.h"
#include "tim.h"

// 表示MPU6050中断已发生的信号量
SemaphoreHandle_t SemMpuIntr;

float Car_Angle_Balance;
uint8_t Flag_Qian, Flag_Hou, Flag_Left, Flag_Right; // 蓝牙遥控相关的变量

/********************************************
以下是平衡小车的PID参数调试整定
*********************************************/

// 平衡小车重要参数调试
// 第一步，确定机械中值
float targetAngle = 4.8; // 机械中值，即小车不上电时候的平衡值。
// 第二部，调试直立环PID
float Vertical_Kp = 444;	 // 平衡环PID 的P值
float Vertical_Kd = 0.81; // 平衡环PID 的D值
// 第三步，调试速度环PID
float Velocity_Kp = -143;		  // 速度环PID 的P值
float Velocity_Ki = -143 / 200.0; // 速度环PID 的I值 = P/200;
// 第四步，调试转向环PID，需要在函数中调节

float Target_Velocity = 40;
float Encoder;
float Encoder_Integral;

void car_task(void *pvParameters)
{
	BaseType_t err = pdFALSE;
	int i = 0;
	int PWMA;
	int PWMB;
	int Encoder_A;
	int Encoder_B;
	int Vertical_Pwm;  // 平衡pwm
	int Velocity_Pwm; // 速度pwm
	int Turn_Pwm;	  // 转向pwm
	// 俯仰角 俯仰角速度 转向角速度
	float Gyro_Balance, Gyro_Turn;

	MPU_Init();
	mpu_dmp_init();

	while (1)
	{
		if (SemMpuIntr != NULL)
		{
			err = xSemaphoreTake(SemMpuIntr, (TickType_t)5); // 获取信号量
			if (err == pdTRUE)								 // 获取信号量成功
			{
				//===读取编码器的值，因为两个电机的旋转了180度的，所以对其中一个取反
				Encoder_A = -Read_Encoder(2);
				Encoder_B = Read_Encoder(3);

				// 获取小车的平衡角度
				Car_Read_Angle(&Gyro_Balance, &Gyro_Turn, &Car_Angle_Balance);

				Vertical_Pwm = Car_Balance(Car_Angle_Balance, Gyro_Balance);
				Velocity_Pwm = Car_Velocity(Encoder_A, Encoder_B);
				if (1 == Flag_Left || 1 == Flag_Right)
				{
					Turn_Pwm = Car_Turn(Encoder_A, Encoder_B, Gyro_Turn); //===转向环PID控制
				}
				else
				{
					Turn_Pwm = 0.4 * Gyro_Turn;
				}

				PWMA = Vertical_Pwm + Velocity_Pwm + Turn_Pwm;
				PWMB = Vertical_Pwm + Velocity_Pwm - Turn_Pwm;

				// 角度太大时小车会倒下，停止电机免得小车在地上乱跑
				if (Car_Angle_Balance >= 35 || Car_Angle_Balance <= -35)
				{
					PWMA = PWMB = 0;
					Flag_Qian = Flag_Hou = Flag_Left = Flag_Right = 0;
					Encoder = Encoder_Integral = 0;
				}

				if (i < 200)
				{ 
					// 刚上电的时候，车子可能是倒下来的状态，此时读出的角度有段时间小于35度，轮子转动会导致车子在桌面上乱跑
					i++;
					Set_Pwm(0, 0);
				}
				else
				{
					Set_Pwm(PWMA, PWMB); // pwm更新
				}
			}
		}
		else if (err == pdFALSE)
		{
			printf("%s, %d, SemMpuIntr is NULL\r\n", __FILE__, __LINE__);
			osDelay(10); // 延时10ms，也就是10个时钟节拍
		}
	}
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	BaseType_t xHigherPriorityTaskWoken;

	if (SemMpuIntr != NULL) // 接收到数据，并且二值信号量有效
	{
		xSemaphoreGiveFromISR(SemMpuIntr, &xHigherPriorityTaskWoken); // 释放二值信号量
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);				  // 如果需要的话进行一次任务切换
	}
}

/********************************************
函数功能：获取角度
*********************************************/
void Car_Read_Angle(float *gyro_balance, float *gyro_turn, float *vertical_angle)
{
	short Gryo[3]; // 设置陀螺仪缓存，测量角速度
	float Gyro_Y, Gyro_Z;
	float Pitch, Roll, Yaw;

	MPU_Get_Gyroscope(&Gryo[0], &Gryo[1], &Gryo[2]); // 获取角速度
	mpu_dmp_get_data(&Pitch, &Roll, &Yaw);			 // dmp角度

	Gyro_Y = (float)Gryo[1]; // Y轴角速度原始
	Gyro_Z = (float)Gryo[2]; // Z轴角速度原始

	if (Gyro_Y > 32768)
		Gyro_Y -= 65536; // 数据类型转换  也可通过short强制类型转换
	if (Gyro_Z > 32768)
		Gyro_Z -= 65536; // 数据类型转换

	Gyro_Y = -Gyro_Y;

	*gyro_balance = Gyro_Y - 5; // 更新平衡角速度, 4为静止状态的飘移
	*gyro_turn = Gyro_Z - 16;   // 更新转向角速度, 16为静止状态的飘移

	*vertical_angle = -Pitch;		  // 直接使用DMP输出的俯仰角
}

/**************************************************************************
函数功能：直立PD控制
入口参数：角度、角速度
返回  值：直立控制PWM
**************************************************************************/
int Car_Balance(float Angle, float Gyro)
{
	float Bias;
	int balance;

	Bias = Angle - targetAngle;						 //===求出平衡的角度中值 和机械相关
	balance = Vertical_Kp * Bias + Gyro * Vertical_Kd; //===计算平衡控制的电机PWM  PD控制

	return balance;
}

/**************************************************************************
函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了
入口参数：左轮编码器、右轮编码器
返回  值：速度控制PWM
**************************************************************************/
int Car_Velocity(int encoder_left, int encoder_right)
{
#define MAX_ENCODER_INT 9000

	float Velocity;
	float Movement;
	float Encoder_Least;

	//=============遥控前进后退部分=======================//
	if (1 == Flag_Qian)
		Movement = -Target_Velocity; //===前进标志位置1
	else if (1 == Flag_Hou)
		Movement = Target_Velocity; //===后退标志位置1
	else
		Movement = 0;

	//=============速度PI控制器=======================//
	Encoder_Least = (encoder_left + encoder_right) - 0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
	Encoder *= 0.8;										//===一阶低通滤波器
	Encoder += Encoder_Least * 0.2;						//===一阶低通滤波器
	Encoder_Integral += Encoder;						//===积分出位移 积分时间：10ms
	Encoder_Integral = Encoder_Integral - Movement;		//===接收遥控器数据，控制前进后退

	//===积分限幅
	if (Encoder_Integral > MAX_ENCODER_INT)
		Encoder_Integral = MAX_ENCODER_INT;

	if (Encoder_Integral < -MAX_ENCODER_INT)
		Encoder_Integral = -MAX_ENCODER_INT;

	Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki; //===速度控制
	return Velocity;
}

/**************************************************************************
函数功能：转向控制  修改转向速度，请修改Turn_Amplitude即可
入口参数：左轮编码器、右轮编码器、Z轴陀螺仪
返回  值：转向控制PWM
**************************************************************************/
int Car_Turn(int encoder_a, int encoder_b, float gyro) // 转向控制
{
	static float Turn_Target, Turn, Encoder_temp, Turn_Convert = 10, Turn_Count;
	float Turn_Amplitude = 100, Kp = 20, Kd = 0;

	//=============遥控左右旋转部分=======================//
	// 这一部分主要是根据旋转前的速度调整速度的起始速度，增加小车的适应性
	if (1 == Flag_Left || 1 == Flag_Right)
	{
		if (++Turn_Count == 1)
			Encoder_temp = myabs(encoder_a + encoder_b);

		Turn_Convert = 55 / Encoder_temp;

		if (Turn_Convert < 0.6)
			Turn_Convert = 0.6;

		if (Turn_Convert > 3)
			Turn_Convert = 3;
	}
	else
	{
		Turn_Convert = 0;
		Turn_Count = 0;
		Encoder_temp = 0;
	}

	if (1 == Flag_Left)
		Turn_Target += Turn_Convert;
	else if (1 == Flag_Right)
		Turn_Target -= Turn_Convert;
	else
		Turn_Target = 0;

	if (Turn_Target > Turn_Amplitude)
		Turn_Target = Turn_Amplitude; //===转向	速度限幅

	if (Turn_Target < -Turn_Amplitude)
		Turn_Target = -Turn_Amplitude;

	if (Flag_Qian == 1 || Flag_Hou == 1)
		Kd = -0.5;
	else
		Kd = 0; // 转向的时候取消陀螺仪的纠正 有点模糊PID的思想

	//=============转向PD控制器=======================//
	Turn = -Turn_Target * Kp - gyro * Kd; //===结合Z轴陀螺仪进行PD控制
	return Turn;
}

/**************************************************************************
函数功能：绝对值函数
入口参数：int
返回  值：unsigned int
**************************************************************************/
int myabs(int a)
{
	int temp;
	if (a < 0)
		temp = -a;
	else
		temp = a;
	return temp;
}

void Set_Pwm(int motoA, int motoB)
{
	uint16_t ccr;
	// printf("%d,%d\r\n", motoA, motoB);

	if (motoA < 0)
	{
		AIN1(0);
		AIN2(1);
	}
	else
	{
		AIN1(1);
		AIN2(0);
	}

	ccr = myabs(motoA) > 7200 ? 7200 : myabs(motoA);
	__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, ccr);

	if (motoB < 0)
	{
		BIN1(1);
		BIN2(0);
	}
	else
	{
		BIN1(0);
		BIN2(1);
	}

	ccr = myabs(motoB) > 7200 ? 7200 : myabs(motoB);
	__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, ccr);
}

int Read_Encoder(uint8_t TIMX)
{
	int Encoder_TIM;
	switch (TIMX)
	{
	case 2:
		Encoder_TIM = (short)__HAL_TIM_GET_COUNTER(&htim2);
		__HAL_TIM_SET_COUNTER(&htim2, 0);
		break;
	case 3:
		Encoder_TIM = (short)__HAL_TIM_GET_COUNTER(&htim3);
		__HAL_TIM_SET_COUNTER(&htim3, 0);
		break;
	default:
		Encoder_TIM = 0;
	}

	return Encoder_TIM;
}
